#!/usr/bin/env python3

import os  
from ament_index_python import get_package_share_directory, get_package_prefix
from launch.launch_description_sources import PythonLaunchDescriptionSource  
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch import LaunchDescription
from launch.substitutions import (
                                Command,
                                PathJoinSubstitution, 
                                PythonExpression, 
                                LaunchConfiguration)  
from launch.actions import (DeclareLaunchArgument, 
                            SetEnvironmentVariable, 
                            IncludeLaunchDescription, 
                            ExecuteProcess, 
                            RegisterEventHandler)
from launch.conditions import IfCondition  
from launch.event_handlers import OnProcessExit
import xacro

def generate_launch_description():  
    bumperbot_description = get_package_share_directory('bumperbot_description')  
    bumperbot_description_prefix = get_package_prefix('bumperbot_description')  
    gazebo_ros_dir = get_package_share_directory('gazebo_ros')  
  
    # model_arg = DeclareLaunchArgument(  
    #     name='model',  
    #     # default_value=os.path.join(bumperbot_description, 'urdf', 'bumperbot.xacro.urdf'),  
    #     default_value=os.path.join(bumperbot_description, 'urdf', 'my_robot.urdf'),  
    #     description='Absolute path to robot URDF file'  
    # )  

    doc = xacro.parse(open(os.path.join(bumperbot_description, 'urdf', 'my_robot.urdf')))
    xacro.process_doc(doc)
    params = {'robot_description': doc.toxml()}

    world_name_arg = DeclareLaunchArgument(name='world_name', default_value='test_world')  
  
    world_path = PathJoinSubstitution([  
        bumperbot_description,  
        'worlds',  
        PythonExpression(expression=["'", LaunchConfiguration("world_name"), "'", " + '.world'"])  
    ])  
  
    model_path = os.path.join(bumperbot_description, 'models')  
    model_path += os.pathsep + os.path.join(bumperbot_description_prefix, 'share')  
  
    env_var = SetEnvironmentVariable(name='GAZEBO_MODEL_PATH', value=model_path)  
    
    robot_description = ParameterValue(Command(['xacro ', LaunchConfiguration('model')]), value_type=str)  
    
    # 启动机器人状态发布者节点  
    robot_state_publisher_node = Node(  
        package='robot_state_publisher',  
        executable='robot_state_publisher',   
        output='screen',  
        # parameters=[{"robot_description": robot_description, "use_sim_time": True}]  
        parameters=[params]
    )

    start_gazebo_server = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(gazebo_ros_dir, "launch", "gzserver.launch.py")),
        launch_arguments={"world": world_path}.items(),
    )

    start_gazebo_client = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(gazebo_ros_dir, "launch", "gzclient.launch.py"))
    )
    
    spawn_robot = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=["-topic", "robot_description", "-entity", "bumperbot",],
        output="screen"
    )

    load_joint_state_broadcaster = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'joint_state_broadcaster'],
        output='screen'
    )

    load_diff_drive_base_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'diff_drive_base_controller'],
        output='screen'
    )
    
    return LaunchDescription([
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=spawn_robot,
                on_exit=[load_joint_state_broadcaster],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=load_joint_state_broadcaster,
                on_exit=[load_diff_drive_base_controller],
            )
        ),
        env_var,
        # model_arg,
        world_name_arg,
        start_gazebo_server,
        start_gazebo_client,
        robot_state_publisher_node,
        spawn_robot,
    ])  
  
